Low-cost tightly coupled GPS/INS integration based on a nonlinear Kalman filtering design

This paper describes the design of a tightly coupled GPS/INS integration system based on nonlinear Kalman filtering methods. The traditional methods include linearization of the system around a nominal trajectory, and the extended Kalman filtering (EKF) method which linearizes the system around the previous estimate, or the predication, whichever is available. The recently proposed sigma-point Kalman filtering (SPKF) method uses a set of weighted samples (sigma points) to completely capture the first and second order moments of the prior random variable. In contrast to the EKF, the SPKF has a simpler implementation as it does not require the Jacobian matrices – the computation of which may lead to analytical or computational problems in some applications. This research is conducted under the Australian Cooperative Research Centre (CRC) for Spatial Information (CRC-SI) project 1.3 “Integrated Positioning and Geo-referencing Platform”. The project aims to develop a generic hardware/software platform for positioning and imaging sensor integration. The current work focuses on development of software and algorithms, and a field programmable gate arrays (FPGA) based GPS/INS data logging system. In the current development phase, a tightly coupled GPS/INS integration system based on a linearization around the INS solution has been designed and implemented. The system uses the GPS pseudorange and Doppler measurements to estimate the INS errors. This paper describes further developments of the integration filter design based on the EKF and SPKF methodologies, in order to compare the performance of nonlinear filtering approaches. Experimental results are presented and further planned developments are outlined. INTRODUCTION The integration of GPS and INS can overcome the defects of INS or GPS standalone systems, and benefits from the complementary characteristics of the two systems. To achieve the highest accuracy, multiple dual-frequency GPS receivers can be used in the integrated system to derive accurate baseline solutions from the carrier phase measurements [1]. However, there are many applications requiring a low-cost medium-precision integration system based on a low-cost GPS receiver and IMU, as for example the guidance and navigation of unmanned vehicles [2]. In the design of such a system, a tightly coupled integration approach is more sophisticated than the loosely coupled one [3]. For example, tight integration uses the GPS pseudorange/Doppler measurements directly, and the INS errors can be continuously corrected even if the number of visible GPS satellites drops to below four. On the other hand, a tightly coupled integration algorithm introduces more nonlinear properties into the system. For instance, a loosely coupled system isolates the nonlinear GPS range/range-rate equations to the GPS navigation calculation module. However, nonlinear terms arising from tight integration need to be carefully considered in the design of the integration Kalman filter. The nonlinear issues usually arise from the range and range-rate measurement equations, the discretization of the INS error model, and the triangular terms associated with the attitude angles. The nonlinear property associated with the attitude matrix also affects the lever arm term if the lever arm is expressed in the body frame system. Most state-of-the-art GPS/INS integration systems are designed to estimate the INS solution errors using the GPS measurement data. The INS error propagation equation is the system equation in the integrated systems. In tightly coupled GPS/INS integration, the GPS range and range-rate data are utilized and the range and rangerate measurement equations are linearized around the INS solution. The standard Kalman filter is then applied to estimate the INS errors. This scheme has been demonstrated by many successful systems and their applications over the past two decades [3][4]. Recently application of nonlinear filtering methods to integrated navigation has been investigated in the literature [2][5]. In these investigations the differential equation of the INS mechanization or the kinematical equations of the host platform is chosen as the system dynamics model. This design unavoidably introduces nonlinearities to the filtering system even in a loosely coupled GPS/INS integration system where the GPS position/velocity solution is directly applied. The extended Kalman filter is the “standard” approach for state estimation of nonlinear systems over the past three decades [6]. The principal idea of the EKF is linearization of the system equation and/or the measurement equation around the previous estimate or the current prediction. The linearized system is then represented by the Jacobians of the nonlinear system/measurement functions. The normal Kalman filtering formulae are applied to the linearized system. The procedure produces the suboptimal estimate of the state of the system. The EKF has some defects, such as difficulty in implementation, difficult to tune, and the first order term is insufficiently accurate to approximate the nonlinearities of the system [7]. The sigma-point Kalman filter was developed to overcome the limitation of the EKF. Distinguishing itself from the normal Kalman filter, the SPKF calculates the filtering parameters by utilization of a set of sampling-like points, the so-called the “sigma points” which can be mapped into the state space or the measurement space through the nonlinear functions of the system directly, instead of linearization through the Jacobians. The parameters derived from the sigma points include the SPKF gain matrix, the state prediction and its covariance, the measurement prediction and its covariance, as well as the estimate covariance [7-9]. However, the EKF calculates the covariance matrices and the Kalman filtering gain matrix using the Jacobians. The EKF is the first order approximation of the nonlinear system and thus may introduce larger errors to the solution, especially if the system has large nonlinearities and the higher order terms are neglected. The SPKF approach is expected to give a better approximation to the nonlinear system because it is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function or transformation [7]. This paper develops a tightly coupled GPS/INS integrated system using the SPKF approach. The research is conducted under the CRC for Spatial Information (CRCSI) project 1.3 “Integrated Positioning and Georeferencing Platform”. The aims of the project include: (1) to develop a generic integrated positioning/georeferencing platform system based on FPGA technology, that can be subsequently reconfigured for optimized positioning and spatial data acquisition applications; and (2) to develop a suite of software that allows for precise, time-synchronized measurement logging, sensor control, real-time data processing, and sundry operations necessary to support such mapping applications. OPTIMAL ESTIMATION FOR NONLINEAR SYSTEMS Consider the nonlinear discrete-time system below ) k ( ) k , 1 k ( ] k ), k ( [ ) 1 k ( w G x f x + + = + (1) ) 1 k ( ] 1 k ), 1 k ( [ ) 1 k ( + + + + = + v x h z (2) where x(k) is the state of the system at k, and z(k) is the measurement vector. The vectors w(k) and v(k) are the system noise and measurement noise respectively. Extended Kalman Filter The EKF applies the Kalman filter to nonlinear systems by simply linearizing all the nonlinear models so that the traditional linear Kalman filter equations can be applied. The extended Kalman filter (EKF) gives the estimate and the covariance [6] )] k 1 k ( ˆ ) 1 k ( )[ 1 k ( ) k 1 k ( ˆ ) 1 k 1 k ( ˆ + − + + + + = + + z z K x x (3) ) k 1 k ( )] 1 k ( ) 1 k ( [ ) 1 k 1 k ( + + + − = + + P H K I P (4) The prediction of the state and its covariance are ] k ), k k ( ˆ [ ) k 1 k ( ˆ x f x = + (5) ) k , 1 k ( ) k ( ) k , 1 k ( ) k , 1 k ( ) k k ( ) k , 1 k ( ) k 1 k ( T T + + + + + = + G Q G F P F P (6) The prediction of measurement is ] 1 k ), k 1 k ( ˆ [ ) k 1 k ( ˆ + + = + x h z (7) The Kalman gain matrix is